#include "ResultTrajectory.h"
namespace ov_eval
{

ResultTrajectory::ResultTrajectory(std::string path_est, std::string path_gt, std::string alignment_method) {

  // Load from file
  Loader::load_data(path_est, est_times, est_poses, est_covori, est_covpos);
  Loader::load_data(path_gt, gt_times, gt_poses, gt_covori, gt_covpos);

  // Debug print amount
  // std::string base_filename1 = path_est.substr(path_est.find_last_of("/\\") + 1);
  // std::string base_filename2 = path_gt.substr(path_gt.find_last_of("/\\") + 1);
  // PRINT_DEBUG("[TRAJ]: loaded %d poses from %s\n",(int)est_times.size(),base_filename1.c_str());
  // PRINT_DEBUG("[TRAJ]: loaded %d poses from %s\n",(int)gt_times.size(),base_filename2.c_str());
  double len_gt = Loader::get_total_length(gt_poses);
  double len_est = Loader::get_total_length(est_poses);
  double ratio = len_est / len_gt;
  if (ratio > 1.1 || ratio < 0.9) {
    //PRINT_WARNING(YELLOW "[TRAJ]: Trajectory is a bad ratio of %.2f length (est %.2f, gt %.2f)\n", ratio, len_est, len_gt);
   // PRINT_WARNING(YELLOW "[TRAJ]: %s\n", path_est.c_str());
  }

  // Intersect timestamps
  
  //AlignUtils::perform_association(0, 0.02, est_times, gt_times, est_poses, gt_poses, est_covori, est_covpos, gt_covori, gt_covpos);

  // Return failure if we didn't have any common timestamps
  if (est_poses.size() < 3) {
   // PRINT_ERROR(RED "[TRAJ]: unable to get enough common timestamps between trajectories.\n" RESET);
   // PRINT_ERROR(RED "[TRAJ]: does the estimated trajectory publish the rosbag timestamps??\n" RESET);
    std::exit(EXIT_FAILURE);
  }

  // Perform alignment of the trajectories
  Eigen::Matrix3d R_ESTtoGT, R_GTtoEST;
  Eigen::Vector3d t_ESTinGT, t_GTinEST;
  double s_ESTtoGT, s_GTtoEST;

  AlignTrajectory::align_trajectory(est_poses, gt_poses, R_ESTtoGT, t_ESTinGT, s_ESTtoGT, alignment_method);
  AlignTrajectory::align_trajectory(gt_poses, est_poses, R_GTtoEST, t_GTinEST, s_GTtoEST, alignment_method);

  // Debug print to the user
  Eigen::Vector4d q_ESTtoGT = ov_core::rot_2_quat(R_ESTtoGT);
  Eigen::Vector4d q_GTtoEST = ov_core::rot_2_quat(R_GTtoEST);

  //PRINT_DEBUG("[TRAJ]: q_ESTtoGT = %.3f, %.3f, %.3f, %.3f | p_ESTinGT = %.3f, %.3f, %.3f | s = %.2f\n", q_ESTtoGT(0), q_ESTtoGT(1),
  //            q_ESTtoGT(2), q_ESTtoGT(3), t_ESTinGT(0), t_ESTinGT(1), t_ESTinGT(2), s_ESTtoGT);
  // PRINT_DEBUG("[TRAJ]: q_GTtoEST = %.3f, %.3f, %.3f, %.3f | p_GTinEST = %.3f, %.3f, %.3f | s =
  // %.2f\n",q_GTtoEST(0),q_GTtoEST(1),q_GTtoEST(2),q_GTtoEST(3),t_GTinEST(0),t_GTinEST(1),t_GTinEST(2),s_GTtoEST);

  // Finally lets calculate the aligned trajectories
  for (size_t i = 0; i < gt_times.size(); i++) {
    Eigen::Matrix<double, 7, 1> pose_ESTinGT, pose_GTinEST;
    pose_ESTinGT.block(0, 0, 3, 1) = s_ESTtoGT * R_ESTtoGT * est_poses.at(i).block(0, 0, 3, 1) + t_ESTinGT;
    pose_ESTinGT.block(3, 0, 4, 1) = ov_core::quat_multiply(est_poses.at(i).block(3, 0, 4, 1), ov_core::Inv(q_ESTtoGT));
    pose_GTinEST.block(0, 0, 3, 1) = s_GTtoEST * R_GTtoEST * gt_poses.at(i).block(0, 0, 3, 1) + t_GTinEST;
    pose_GTinEST.block(3, 0, 4, 1) = ov_core::quat_multiply(gt_poses.at(i).block(3, 0, 4, 1), ov_core::Inv(q_GTtoEST));
    est_poses_aignedtoGT.push_back(pose_ESTinGT);
    gt_poses_aignedtoEST.push_back(pose_GTinEST);
  }
}

void ResultTrajectory::calculate_ate(Statistics &error_ori, Statistics &error_pos) {

  // Clear any old data
  error_ori.clear();
  error_pos.clear();

  // Calculate the position and orientation error at every timestep
  for (size_t i = 0; i < est_poses_aignedtoGT.size(); i++) {

    // Calculate orientation error
    Eigen::Matrix3d e_R = ov_core::quat_2_Rot(est_poses_aignedtoGT.at(i).block(3, 0, 4, 1)).transpose() *
                          ov_core::quat_2_Rot(gt_poses.at(i).block(3, 0, 4, 1));
    double ori_err = 180.0 / M_PI * ov_core::log_so3(e_R).norm();

    // Calculate position error
    double pos_err = (gt_poses.at(i).block(0, 0, 3, 1) - est_poses_aignedtoGT.at(i).block(0, 0, 3, 1)).norm();

    // Append this error!
    error_ori.timestamps.push_back(est_times.at(i));
    error_ori.values.push_back(ori_err);
    error_pos.timestamps.push_back(est_times.at(i));
    error_pos.values.push_back(pos_err);
  }

  // Update stat information
  error_ori.calculate();
  error_pos.calculate();
}
}

